#!/usr/bin/env python
# Software License Agreement (BSD License)
#
# Copyright (c) 2012, Willow Garage, Inc.
# All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
#  * Redistributions of source code must retain the above copyright
#    notice, this list of conditions and the following disclaimer.
#  * Redistributions in binary form must reproduce the above
#    copyright notice, this list of conditions and the following
#    disclaimer in the documentation and/or other materials provided
#    with the distribution.
#  * Neither the name of Willow Garage, Inc. nor the names of its
#    contributors may be used to endorse or promote products derived
#    from this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.

import rospy

from rosapi import proxy, objectutils, params
from rosapi.glob_helper import get_globs
from rosapi.srv import *
from rosapi.msg import *


# Initialises the ROS node
def register_services():
    rospy.Service('/rosapi/topics', Topics, get_topics)
    rospy.Service('/rosapi/topics_for_type', TopicsForType, get_topics_for_type)
    rospy.Service('/rosapi/services', Services, get_services)
    rospy.Service('/rosapi/services_for_type', ServicesForType, get_services_for_type)
    rospy.Service('/rosapi/nodes', Nodes, get_nodes)
    rospy.Service('/rosapi/node_details', NodeDetails, get_node_details)
    rospy.Service('/rosapi/action_servers', GetActionServers, get_action_servers)
    rospy.Service('/rosapi/topic_type', TopicType, get_topic_type)
    rospy.Service('/rosapi/service_type', ServiceType, get_service_type)
    rospy.Service('/rosapi/publishers', Publishers, get_publishers)
    rospy.Service('/rosapi/subscribers', Subscribers, get_subscribers)
    rospy.Service('/rosapi/service_providers', ServiceProviders, get_service_providers)
    rospy.Service('/rosapi/service_node', ServiceNode, get_service_node)
    rospy.Service('/rosapi/service_host', ServiceHost, get_service_host)
    rospy.Service('/rosapi/message_details', MessageDetails, get_message_details)
    rospy.Service('/rosapi/service_request_details', ServiceRequestDetails, get_service_request_details)
    rospy.Service('/rosapi/service_response_details', ServiceResponseDetails, get_service_response_details)
    rospy.Service('/rosapi/set_param', SetParam, set_param)
    rospy.Service('/rosapi/get_param', GetParam, get_param)
    rospy.Service('/rosapi/has_param', HasParam, has_param)
    rospy.Service('/rosapi/search_param', SearchParam, search_param)
    rospy.Service('/rosapi/delete_param', DeleteParam, delete_param)
    rospy.Service('/rosapi/get_param_names', GetParamNames, get_param_names)
    rospy.Service('/rosapi/get_time', GetTime, get_time)
    rospy.Service('/rosapi/topics_and_raw_types', TopicsAndRawTypes, get_topics_and_raw_types)

def get_topics(request):
    """ Called by the rosapi/Topics service. Returns a list of all the topics being published. """
    topics, types = proxy.get_topics_and_types(rosapi.glob_helper.topics_glob)
    return TopicsResponse(topics, types)

def get_topics_for_type(request):
    """ Called by the rosapi/TopicsForType service. Returns a list of all the topics that are publishing a given type """
    return TopicsForTypeResponse(proxy.get_topics_for_type(request.type, rosapi.glob_helper.topics_glob))

def get_services(request):
    """ Called by the rosapi/Services service. Returns a list of all the services being advertised. """
    return ServicesResponse(proxy.get_services(rosapi.glob_helper.services_glob))

def get_services_for_type(request):
    """ Called by the rosapi/ServicesForType service. Returns a list of all the services that are publishing a given type """
    return ServicesForTypeResponse(proxy.get_services_for_type(request.type, rosapi.glob_helper.services_glob))

def get_nodes(request):
    """ Called by the rosapi/Nodes service. Returns a list of all the nodes that are registered """
    return NodesResponse(proxy.get_nodes())
    
def get_node_details(request):
    """ Called by the rosapi/Nodes service. Returns a node description """
    node = request.node
    return NodeDetailsResponse(proxy.get_node_subscriptions(node), proxy.get_node_publications(node), proxy.get_node_services(node))
    
def get_action_servers(request):
    """ Called by the rosapi/GetActionServers service. Returns a list of action servers based on actions standard topics """
    topics, types = proxy.get_topics_and_types(rosapi.glob_helper.topics_glob)
    action_servers = proxy.filter_action_servers(topics)
    return GetActionServersResponse(action_servers)
    
def get_topic_type(request):
    """ Called by the rosapi/TopicType service.  Given the name of a topic, returns the name of the type of that topic.
    Request class has one field, 'topic', which is a string value (the name of the topic)
    Response class has one field, 'type', which is a string value (the type of the topic)
    If the topic does not exist, an empty string is returned. """
    return TopicTypeResponse(proxy.get_topic_type(request.topic, rosapi.glob_helper.topics_glob))
    
def get_service_type(request):
    """ Called by the rosapi/ServiceType service.  Given the name of a service, returns the type of that service
    Request class has one field, 'service', which is a string value (the name of the service)
    Response class has one field, 'type', which is a string value (the type of the service)
    If the service does not exist, an empty string is returned. """
    return ServiceTypeResponse(proxy.get_service_type(request.service, rosapi.glob_helper.services_glob))

def get_publishers(request):
    """ Called by the rosapi/Publishers service.  Given the name of a topic, returns a list of node names
    that are publishing on that topic. """
    return PublishersResponse(proxy.get_publishers(request.topic, rosapi.glob_helper.topics_glob))

def get_subscribers(request):
    """ Called by the rosapi/Subscribers service.  Given the name of a topic, returns a list of node names
    that are subscribing to that topic. """
    return SubscribersResponse(proxy.get_subscribers(request.topic, rosapi.glob_helper.topics_glob))

def get_service_providers(request):
    """ Called by the rosapi/ServiceProviders service.  Given the name of a topic, returns a list of node names
    that are advertising that service type """
    return ServiceProvidersResponse(proxy.get_service_providers(request.service, rosapi.glob_helper.services_glob))

def get_service_node(request):
    """ Called by the rosapi/ServiceNode service.  Given the name of a service, returns the name of the node
    that is providing that service. """
    return ServiceNodeResponse(proxy.get_service_node(request.service))

def get_service_host(request):
    """ Called by the rosapi/ServiceNode service.  Given the name of a service, returns the name of the machine
    that is hosting that service. """
    return ServiceHostResponse(proxy.get_service_host(request.service))

def get_message_details(request):
    """ Called by the rosapi/MessageDetails service.  Given the name of a message type, returns the TypeDef
    for that type."""
    typedefs = [dict_to_typedef(d) for d in objectutils.get_typedef_recursive(request.type)]
    return MessageDetailsResponse(typedefs)

def get_service_request_details(request):
    """ Called by the rosapi/ServiceRequestDetails service. Given the name of a service type, returns the TypeDef
    for the request message of that service type. """
    return ServiceRequestDetailsResponse([dict_to_typedef(d) for d in objectutils.get_service_request_typedef_recursive(request.type)])

def get_service_response_details(request):
    """ Called by the rosapi/ServiceResponseDetails service. Given the name of a service type, returns the TypeDef
    for the response message of that service type. """
    return ServiceResponseDetailsResponse([dict_to_typedef(d) for d in objectutils.get_service_response_typedef_recursive(request.type)])

def set_param(request):
    rosapi.params.set_param(request.name, request.value, rosapi.glob_helper.params_glob)
    return SetParamResponse()

def get_param(request):
    return GetParamResponse(rosapi.params.get_param(request.name, request.default, rosapi.glob_helper.params_glob))

def has_param(request):
    return HasParamResponse(rosapi.params.has_param(request.name, rosapi.glob_helper.params_glob))

def search_param(request):
    return SearchParamResponse(rosapi.params.search_param(request.name, rosapi.glob_helper.params_glob))

def delete_param(request):
    rosapi.params.delete_param(request.name, rosapi.glob_helper.params_glob)
    return DeleteParamResponse()

def get_param_names(request):
    return GetParamNamesResponse(rosapi.params.get_param_names(rosapi.glob_helper.params_glob))

def get_time(request):
    return GetTimeResponse(rospy.get_rostime())

def get_topics_and_raw_types(request):
    """ Called by the rosapi/topics_and_raw_types service. Returns a list of all the topics being published, and their
    raw types, similar to `gendeps --cat`. """
    topics, types = proxy.get_topics_and_types(rosapi.glob_helper.topics_glob)
    return TopicsAndRawTypesResponse(topics, types, [objectutils.get_typedef_full_text(type) for type in types])

def dict_to_typedef(typedefdict):
    typedef = TypeDef()
    typedef.type = typedefdict["type"]
    typedef.fieldnames = typedefdict["fieldnames"]
    typedef.fieldtypes = typedefdict["fieldtypes"]
    typedef.fieldarraylen = typedefdict["fieldarraylen"]
    typedef.examples = typedefdict["examples"]
    typedef.constnames = typedefdict["constnames"]
    typedef.constvalues = typedefdict["constvalues"]
    return typedef

if __name__ == '__main__':
     try:
         rospy.init_node('rosapi_node')
         get_globs()
         register_services()
         rospy.loginfo("Rosapi started")
         rospy.spin()
     except rospy.ROSInterruptException: 
         pass
